/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_waitfor_release.cpp
 * @Description  : 实现了一个等待释放操作的类OperationWaitForRelease，用于处理AGV在操作过程中等待按钮释放的逻辑。
 *                 主要功能包括：
 *                  - 初始化操作，检查是否需要延迟释放。
 *                  - 检查操作是否准备好启动。
 *                  - 开始等待释放的操作。
 *                  - 定义工作逻辑，通过检查按钮是否被长按来判断是否触发释放事件。
 *                  - 结束操作，根据配置的延迟时间判断是否需要等待。
 *                  - 设置绿灯闪烁以指示释放事件。
 *                  - 更新输入信息以获取当前按钮状态。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:45:06
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation_waitfor_release.h" 
#include <vector>
#include <std_srvs/srv/trigger.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <ros_callbacks.h>
#define SET_ONE_BIT(x) (((uint64_t)1) << (x))
#define TEST_BIT(x, i) ((x & SET_ONE_BIT(i)) != 0)
constexpr int MAX_PREEES_COUNT = 2; // 4*50 = 200ms
constexpr const char* KEY_RELEASE_TIME = "timeDelayRelease";
constexpr const char* SRV_SET_GREEN_BLINK = "/green_light_blink";
constexpr const char* KEY_RELEASE_BUTTON = "releaseButton";

/**
 * @brief 类的构造函数
 *
 * 该构造函数用于初始化 OperationWaitForRelease 对象。
 *
 * @param action agv_msgs::msg::Action 类型的动作参数，用于初始化基类 Operation 对象。
 */
OperationWaitForRelease::OperationWaitForRelease(const agv_msgs::msg::Action& action)
    : Operation(action), pressed_count_(0), time_delay_release_(0)
{
}

/**
 * @brief 初始化函数
 *
 * 此函数用于初始化OperationWaitForRelease对象。
 *
 * @return 如果初始化成功，则返回true；否则返回false。
 */
bool OperationWaitForRelease::Init()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    // 复位定时器
    trigger_once_.ClearTrigger();

    // 是否需要延迟释放
    FindTimeDelayRelease();

    // 放行按钮定义
    agv_msgs::msg::PairTypeInt io;
    io.value = 0;
    io.key = KEY_RELEASE_BUTTON;
    release_button_.Update(io);
   
    client_green_blink_ = ROSNode()->create_client<std_srvs::srv::Trigger>(SRV_SET_GREEN_BLINK);

    while (!client_green_blink_->wait_for_service(std::chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            DEBUG_ERROR_OUT("Interrupted while waiting for the service. Exiting.");
            return false;
        }
        DEBUG_OUT("green_light_blink service not available, waiting again...");
    }
    return true;
}

/**
 * @brief 等待释放的启动准备函数
 *
 * 检查当前操作是否准备好启动。
 *
 * @return 如果操作准备好启动，则返回 true；否则返回 false。
 */
bool OperationWaitForRelease::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif

	// if (!TryCycleMarkCheck())
	// {
	// 	DEBUG_ERROR_OUT("OperationTurnAroundOnce: qrId is not Valid");
	// 	return false;
	// }

    DEBUG_OUT("wait for release ReadyStart");
    return true;
}

/**
 * @brief 开始操作等待释放的动作
 *
 * 该函数用于启动操作等待释放的动作。
 *
 * @return 返回true表示操作成功开始
 */
bool OperationWaitForRelease::StartAction()
{
    std::lock_guard<std::mutex> locker(mutex_);
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    DEBUG_OUT("OperationWaitForRelease start");
    return true;
}

/**
 * @brief 工作实现函数
 *
 * @return 则返回true；否则返回false。
 */
bool OperationWaitForRelease::Work()
{
    std::lock_guard<std::mutex> locker(mutex_);
#ifdef AGV_DEBUG_NAV
    return m_uTotalCount++ > 100; // 5s-500
#endif

    bool release = GetButtonRelease();
    if (release) {
        SetGreenBlink();
        // 开启定时器
        trigger_once_.Start();
        DEBUG_OUT("GetButtonRelease work ok");
    }

    return release;
}

/**
 * @brief 结束等待释放操作
 * 根据time_delay_release_的值判断是否执行等待操作。
 * 如果time_delay_release_不为0，则等待time_delay_release_秒。
 * 如果在等待时间内被触发，则返回true，否则返回false。
 * 如果time_delay_release_为0，则直接返回true。
 *
 * @return 如果成功结束等待释放操作，则返回true；否则返回false。
 */
bool OperationWaitForRelease::EndAction()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    bool ret = false;
    DEBUG_OUT("OperationWaitForRelease EndAction ok");

    if (time_delay_release_ != 0) {
        DEBUG_OUT("wait timeDelayRelease:m_timeDelayRelease(s)=" << time_delay_release_);
        if (trigger_once_.TriggerAlways(time_delay_release_ * 1000)) {
            ret = true;
            DEBUG_OUT("timeDelayRelease:ok");
        }
    } else {
        ret = true;
        DEBUG_OUT("timeDelayRelease is 0");
    }

    return ret;
}

/**
 * @brief 获取按钮释放状态
 *
 * 获取按钮是否已被释放，并根据一定的逻辑判断是否真正触发释放事件。
 *
 * @return 如果按钮已释放且被判定为真正触发，则返回true；否则返回false。
 */
bool OperationWaitForRelease::GetButtonRelease()
{
    bool ret = false;
    // 如果数据有效，判断是否触发
    const auto &op_data = GetOpData();
    auto msg = op_data.input_new_io_msg;
    // 话题是否超时
    bool timeout = msg.TimeOut(100);
    // 是否获取到数据
    bool ok = UpdateInutIoNew(msg.Get(), release_button_);

    bool cur_pressed = release_button_.Get().value;
    if (timeout || !ok || !cur_pressed) {
        pressed_count_ = 0;
        DEBUG_OUT("GetButtonRelease:timeout=" << timeout << ",curPressed=" << cur_pressed
                                              << ",getOk=" << ok);
        return ret;
    }

    // 如果按钮长按下，超过周期阈值，则认为是真正触发
    if (cur_pressed) {
        if (++pressed_count_ >= MAX_PREEES_COUNT) {
            pressed_count_ = MAX_PREEES_COUNT;
            ret = true;
            DEBUG_OUT("GetButtonRelease ok ");
        }
    }

    return ret;
}

/**
 * @brief 查找是否设置了延迟释放时间
 *
 * 检查操作参数中是否设置了需要延迟释放的时间，并更新成员变量time_delay_release_。
 *
 * @return 如果找到了需要延迟释放的时间，则返回true；否则返回false。
 */
bool OperationWaitForRelease::FindTimeDelayRelease()
{
    bool ret = false;
    // 查看是否设置需要延迟退出
    auto params = action_.actionparameters;
    if (params.empty()) {
        time_delay_release_ = 0;
        DEBUG_OUT("params is empty; can not need timeDelayRelease");
        return ret;
    }

    for (auto param : params) {
        if (param.key != KEY_RELEASE_TIME)
            continue;

        if (string2Number(param.value, time_delay_release_)) {
            DEBUG_OUT("FindTimeDelayRelease OK;m_timeDelayRelease=" << time_delay_release_);
            ret = true;
        }
        break;
    }

    return ret;
}

/**
 * @brief 设置绿灯闪烁
 *
 * 通过服务调用使绿灯闪烁一次。
 *
 * @return 返回操作是否成功，成功返回true，否则返回false。
 */
bool OperationWaitForRelease::SetGreenBlink()
{
    auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
    using ServiceResponseFuture = rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture;
    auto response_received_callback = [](ServiceResponseFuture future) {
        DEBUG_OUT("SetGreenBlink once " << "success=" << future.get()->success
                        << ",message=" << future.get()->message);
    };

    auto result = client_green_blink_->async_send_request(request, response_received_callback);

    if (result.valid()) {
        DEBUG_OUT("Request successfully sent");
        return true;
    } else {
        DEBUG_OUT("Failed to send request");
        return false;
    }
}

/**
 * @brief 更新输入信息
 *
 * 根据传入的消息更新输入信息。
 *
 * @param msg 输入消息，包含需要更新的输入信息
 * @param io 需要更新的输入信息
 *
 * @return 如果找到匹配的输入信息并成功更新，则返回true；否则返回false
 */
bool OperationWaitForRelease::UpdateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &io)
{
    bool ret = false;
    auto input_io = io.Get();
    std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
    for (auto &data : inputs_msg) {
        if (data.key == input_io.key) {
            io.Update(data);
            ret = true;
            break;
        }
    }

    return ret;
}